#ifndef EGO_PLANNER_ADAPTER__BSPLINE_INTERPOLATOR_HPP_
#define EGO_PLANNER_ADAPTER__BSPLINE_INTERPOLATOR_HPP_

#include <Eigen/Dense>
#include <vector>
#include <memory>

// Forward declaration for ego-planner messages
namespace traj_utils {
namespace msg {
class Bspline;
}
}

namespace ego_planner_adapter {

/**
 * @brief B-spline trajectory interpolation utilities
 */
class BSplineInterpolator {
public:
    BSplineInterpolator();
    ~BSplineInterpolator() = default;
    
    /**
     * @brief Set trajectory from ego-planner B-spline message
     * @param bspline_msg B-spline trajectory message
     * @return true if trajectory is valid and set successfully
     */
    bool setTrajectory(const std::shared_ptr<const traj_utils::msg::Bspline>& bspline_msg);
    
    /**
     * @brief Get position at given time
     * @param time Time relative to trajectory start
     * @param position Output position vector
     * @return true if time is within trajectory bounds
     */
    bool getPositionAtTime(double time, Eigen::Vector3d& position);
    
    /**
     * @brief Get velocity at given time
     * @param time Time relative to trajectory start
     * @param velocity Output velocity vector
     * @return true if time is within trajectory bounds
     */
    bool getVelocityAtTime(double time, Eigen::Vector3d& velocity);
    
    /**
     * @brief Get yaw angle at given time
     * @param time Time relative to trajectory start
     * @param yaw Output yaw angle
     * @return true if time is within trajectory bounds
     */
    bool getYawAtTime(double time, double& yaw);
    
    /**
     * @brief Get trajectory start time
     */
    double getStartTime() const { return start_time_; }
    
    /**
     * @brief Get trajectory end time
     */
    double getEndTime() const { return end_time_; }
    
    /**
     * @brief Check if trajectory is valid
     */
    bool isValid() const { return is_valid_; }
    
    /**
     * @brief Clear current trajectory
     */
    void clear();

private:
    /**
     * @brief Evaluate B-spline at parameter t
     * @param t Parameter value (0 to 1)
     * @return Position at parameter t
     */
    Eigen::Vector3d evaluateBSpline(double t);
    
    /**
     * @brief Evaluate B-spline derivative at parameter t
     * @param t Parameter value (0 to 1)
     * @return Velocity at parameter t
     */
    Eigen::Vector3d evaluateBSplineDerivative(double t);
    
    /**
     * @brief Convert time to parameter value
     * @param time Time relative to trajectory start
     * @return Parameter value (0 to 1)
     */
    double timeToParameter(double time);
    
    /**
     * @brief Compute B-spline basis function
     * @param i Control point index
     * @param k Degree
     * @param t Parameter value
     * @return Basis function value
     */
    double basisFunction(int i, int k, double t);
    
    /**
     * @brief Compute B-spline basis function derivative
     * @param i Control point index
     * @param k Degree
     * @param t Parameter value
     * @return Basis function derivative value
     */
    double basisFunctionDerivative(int i, int k, double t);
    
    // Trajectory data
    std::vector<Eigen::Vector3d> control_points_;
    std::vector<double> knots_;
    std::vector<double> yaw_points_;
    double yaw_dt_;
    double start_time_;
    double end_time_;
    int order_;
    bool is_valid_;
    
    // Internal parameters
    int num_control_points_;
    int num_knots_;
    double trajectory_duration_;
    
    // Constants
    static constexpr double EPSILON = 1e-9;
    static constexpr int MAX_ITERATIONS = 100;
};

} // namespace ego_planner_adapter

#endif // EGO_PLANNER_ADAPTER__BSPLINE_INTERPOLATOR_HPP_